//#include "scorbot.h"

//standard IO:
#include <sys/select.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

//Socket headers:
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>

//for threading/locks:
#include <pthread.h>

//serial port connections:
#include <termios.h>
#include <fcntl.h>

#include "queue.h"

#define DEBUG 0

//the serial port's ID number
int serial_fd = -1;

QUEUE_HANDLE scorbotWrite_handle;	//From X to Write thread
QUEUE_HANDLE scorbotRead_handle;	//From write thread to read thread
QUEUE_HANDLE scorbotOutput_handle;	//from read thread to X

void* readFromScorbot()	{
    char toSend[600];
	char readbuf[600];
    char c = '\0';
    int num = 0, max = 599, text = 0;
    QUEUE_ELEM fromWrite;
    QUEUE_ELEM output;
    fd_set rdset;
    struct timeval timeout;
    
    while(1)	{
		text = 0;
		num = 0;

		memset(&toSend, 0, sizeof(toSend));
		memset(&fromWrite, 0, sizeof(fromWrite));
		memset(&output, 0, sizeof(output));

		queue_pop(scorbotRead_handle, &fromWrite);

		if (DEBUG == 1)	{
			printf("\n\nREADING:\n");
		}

		FD_ZERO(&rdset);
		FD_SET(serial_fd, &rdset);
		timeout.tv_sec  = fromWrite.rw.expectedTime;
		timeout.tv_usec = 0;
		/* NOTE: select() on Linux updates the timeout value
		 * to reflect the time NOT slept. Thus, the total timeout
		 * across all calls to select will be expectedTime, not
		 * the timeout for each specific one. 
		 */

		tcflush(serial_fd, TCIOFLUSH);

		// Ignore initial lines
//		printf("Ignoring %d line(s) -- timeout = %d:\n", fromWrite.rw.ignoreNumLines, (int) timeout.tv_sec);
		while (fromWrite.rw.ignoreNumLines > 0 && 
			   select(serial_fd+1, &rdset, NULL, NULL, &timeout) > 0) {
			int len;
			len = read(serial_fd, &readbuf, 599);
			readbuf[len] = '\0';
//			printf("Ignoring: %s\n", readbuf);
			fromWrite.rw.ignoreNumLines--;
			FD_ZERO(&rdset);
			FD_SET(serial_fd, &rdset);
			memset(readbuf, 0, 600);
		}
//		printf("Done ignoring.\nStarting to read actual lines:\n");

		// Read until terminator string is found or expectedLines reached
		FD_ZERO(&rdset);
		FD_SET(serial_fd, &rdset);
		while (fromWrite.rw.expectedLines > 0 && 
			   select(serial_fd+1, &rdset, NULL, NULL, &timeout) > 0) {
			// Data is available
			int len = read(serial_fd, readbuf, 599);
			readbuf[len-1] = ' ';	//gets rid of newline
			fromWrite.rw.expectedLines--; //hack - maybe change
//			printf("Read: %s\n", readbuf);
			num += len;

			// Copy into toSend
			strncat(toSend, readbuf, len);
		}

		//push toSend into the queue
		strncpy(output.data, toSend, num);
		if (DEBUG == 1)
			printf("Pushing \"%s\" onto queue\n", output.data);

		// Flush input stream in case we didn't get everything
		tcflush(serial_fd, TCIOFLUSH);	//also flushes everything because write shouldn't have written yet

		queue_push(scorbotOutput_handle, &output);
    }
}

void* writeToScorbot()	{
    if (DEBUG == 1)	{
		printf("writeToScorbot thread created, doing work:\n");
    }

    int num = 0;
    QUEUE_ELEM toWrite;

    while(1)	{
		//get data from queue into toSend
		queue_pop(scorbotWrite_handle, &toWrite);
		tcflush(serial_fd, TCIOFLUSH);		//new write is coming, last read must have finished
	
		if (DEBUG == 1)	{
			printf("\n\nWRITING:\n");
			printf("Popped \"%s\" off of write queue\n", toWrite.rw.cmd);
			printf("Writing %s to serial port:\n", toWrite.rw.cmd);
		}

		int sizeSend = strlen(toWrite.rw.cmd)+2;
		int length = strlen(toWrite.rw.cmd);
		char input[sizeSend];
		bzero(input,sizeSend);

		strcpy(input, toWrite.rw.cmd);

		input[length] = '\r';
		input[length+1] = '\0';

		int ret = write(serial_fd, input, strlen(input));

		if (DEBUG == 1)	{
			fprintf(stdout, "WROTE to the serial port: %s\n", input);
		}

		tcdrain(serial_fd);
		queue_push(scorbotRead_handle, &toWrite);
    }
}

//Creates the connection threads for communicating with 
int initializeConnection()	{
    struct termios scorbot_config; //sets up termios configuration structure for the scorbot serial port -go
	
    const char *device = "/dev/ttyUSB0"; //sets where the serial port is plugged in -go
    serial_fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY); //opens serial port in device slot -go
    if(serial_fd == -1) {
		fprintf(stdout, "failed to open serial port\n");
		return -1;
    }
	
	bzero(&scorbot_config, sizeof(scorbot_config)); /* clear struct for new port settings */
	
	
	/* 
	  BAUDRATE: Set bps rate. You could also use cfsetispeed and cfsetospeed.
	  CS8     : 8n1 (8bit,no parity,1 stopbit)
	  CLOCAL  : local connection, no modem contol
	  CREAD   : enable receiving characters
	*/
	scorbot_config.c_cflag = B9600 | CS8 | CLOCAL | CREAD;
	 
	/*
	  IGNPAR  : ignore bytes with parity errors
	  ICRNL   : map CR to NL (otherwise a CR input on the other computer
		        will not terminate input)
	  IGNCR   : ignore carriage returns
	  otherwise make device raw (no other input processing)
	*/
	scorbot_config.c_iflag = IGNPAR | ICRNL | IGNCR ;//| IXON | IXOFF;
	 
	/*
	 Raw output.
	*/
	scorbot_config.c_oflag = 0;
	 
	/*
	  ICANON  : enable canonical input
	  disable all echo functionality, and don't send signals to calling program
	*/
	scorbot_config.c_lflag = ICANON;
	 
	/* 
	  initialize all control characters 
	  default values can be found in /usr/include/termios.h, and are given
	  in the comments, but we don't need them here
	*/
	scorbot_config.c_cc[VINTR]    = 0;     /* Ctrl-c */ 
	scorbot_config.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
	scorbot_config.c_cc[VERASE]   = 0;     /* del */
	scorbot_config.c_cc[VKILL]    = 0;     /* @ */
	scorbot_config.c_cc[VEOF]     = 4;     /* Ctrl-d */
	scorbot_config.c_cc[VTIME]    = 0;     /* inter-character timer unused */
	scorbot_config.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives */
	scorbot_config.c_cc[VSWTC]    = 0;     /* '\0' */
	scorbot_config.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
	scorbot_config.c_cc[VSTOP]    = 0;     /* Ctrl-s */
	scorbot_config.c_cc[VSUSP]    = 0;     /* Ctrl-z */
	scorbot_config.c_cc[VEOL]     = 0;     /* '\0' */
	scorbot_config.c_cc[VREPRINT] = 0;     /* Ctrl-r */
	scorbot_config.c_cc[VDISCARD] = 0;     /* Ctrl-u */
	scorbot_config.c_cc[VWERASE]  = 0;     /* Ctrl-w */
	scorbot_config.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
	scorbot_config.c_cc[VEOL2]    = 0;     /* '\0' */

	/* 
	  now clean the modem line and activate the settings for the port
	*/
	tcflush(serial_fd, TCIOFLUSH);
	tcsetattr(serial_fd,TCSANOW,&scorbot_config);

    return 0;
}

int commDriver_Init() {
    pthread_t writer, reader;

    //initializeConnection to connect to the scorbot via USB to serial port
    if(initializeConnection() == -1)	{
		printf("ERROR on initialize!\n");
		return -1;
	}

    // Initialize queues
    scorbotWrite_handle  = init_queue();
    scorbotRead_handle   = init_queue();
    scorbotOutput_handle = init_queue();

    // Initialize threads:
    pthread_create(&reader, NULL, readFromScorbot, NULL);
    pthread_detach(reader);

    pthread_create(&writer, NULL, writeToScorbot, NULL);
    pthread_detach(writer);

    return 0;
}

void commDriver_Uninit() {
    close(serial_fd);
}
